
#include <sbus.h>
#include <control.h>
#include <rtthread.h>
#include <math.h>
#include <bsp_led.h>
#include <pid.h>
#include <filter.h>

// static struct attitude_euler attitude_error;        /* 目标姿态角和当前姿态角的误差 */
// static struct attitude_euler_angular angular_error; /* 目标姿态角速度和当前姿态角的误差 */
// static rt_time_t last_pid_timestamp = 0;            /* 记录上一次PID控制时间戳 */

static enum uav_status uav_armed = lock;  /* 无人机处于解锁状态，初始上电时处于未解锁状态，可以通过遥控器的通道6进行解锁 */
static biquadFilter_t biquad_pitch_ditem; /* 对于俯仰微分项的滤波器 */
static biquadFilter_t biquad_roll_ditem;  /* 对于滚转微分项的滤波器 */
static biquadFilter_t biquad_yaw_ditem;   /* 对于偏航微分项的滤波器 */

static float pid_filter_pitch_rate(float in)
{
    return biquad_filter(&biquad_pitch_ditem, in);
}

static float pid_filter_roll_rate(float in)
{
    return biquad_filter(&biquad_roll_ditem, in);
}

static float pid_filter_yaw_rate(float in)
{
    return biquad_filter(&biquad_yaw_ditem, in);
}

/* 滚转角控制，根据测量的滚转角和目标滚转角输出期望滚转角速度 */
static pid_control_t roll_control = {
    .kp = ROLL_ERROR_KP_ROLL_ANGULAR,
    .ki = 0,
    .kd = 0,
    .integral_sum = 0,
    .integral_sum_min = 0,
    .integral_sum_max = 0,
    .output_min = -EXPECT_ROLL_ANGULAR_MAX_DEGREE,
    .output_max = EXPECT_ROLL_ANGULAR_MAX_DEGREE,
    .filter = 0};

/* 滚转角速度控制，根据测量的滚转角速度和目标滚转角速度输出期望广义控制量 */
static pid_control_t roll_angular_control = {
    .kp = ROLL_ANGULAR_KP,
    .ki = ROLL_ANGULAR_KI,
    .kd = ROLL_ANGULAR_KD,
    .integral_sum = 0,
    .integral_sum_min = -ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX,
    .integral_sum_max = ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX,
    .output_min = -ATTITUDE_CONTROL_OUTPUT_MAX,
    .output_max = ATTITUDE_CONTROL_OUTPUT_MAX,
    .filter = pid_filter_roll_rate};

/* 俯仰角控制，根据测量的俯仰角和目标俯仰角输出期望俯仰角速度 */
static pid_control_t pitch_control = {
    .kp = ROLL_ERROR_KP_ROLL_ANGULAR,
    .ki = 0,
    .kd = 0,
    .integral_sum = 0,
    .integral_sum_min = 0,
    .integral_sum_max = 0,
    .output_min = -EXPECT_PITCH_ANGULAR_MAX_DEGREE,
    .output_max = EXPECT_PITCH_ANGULAR_MAX_DEGREE,
    .filter = 0};

/* 俯仰角速度控制，根据测量的俯仰角速度和目标俯仰角速度输出期望广义控制量 */
static pid_control_t pitch_angular_control = {
    .kp = PITCH_ANGULAR_KP,
    .ki = PITCH_ANGULAR_KI,
    .kd = PITCH_ANGULAR_KD,
    .integral_sum = 0,
    .integral_sum_min = -ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX,
    .integral_sum_max = ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX,
    .output_min = -ATTITUDE_CONTROL_OUTPUT_MAX,
    .output_max = ATTITUDE_CONTROL_OUTPUT_MAX,
    .filter = pid_filter_pitch_rate};

/* 偏航角控制，根据测量的偏航角和目标偏航角输出期望偏航角速度 */
static pid_control_t yaw_control = {
    .kp = YAW_ERROR_KP_YAW_ANGULAR,
    .ki = 0,
    .kd = 0,
    .integral_sum = 0,
    .integral_sum_min = 0,
    .integral_sum_max = 0,
    .output_min = -EXPECT_YAW_ANGULAR_MAX_DEGREE,
    .output_max = EXPECT_YAW_ANGULAR_MAX_DEGREE,
    .filter = 0};

/* 偏航角速度控制，根据测量的偏航角速度和目标偏航角速度输出期望广义控制量 */
static pid_control_t yaw_angular_control = {
    .kp = YAW_ANGULAR_KP,
    .ki = YAW_ANGULAR_KI,
    .kd = YAW_ANGULAR_KD,
    .integral_sum = 0,
    .integral_sum_min = -ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX,
    .integral_sum_max = ATTITUDE_CONTROL_OUTPUT_INTEGRAL_MAX,
    .output_min = -ATTITUDE_CONTROL_OUTPUT_MAX,
    .output_max = ATTITUDE_CONTROL_OUTPUT_MAX,
    .filter = pid_filter_yaw_rate};

// /// @brief 向量归一化
// /// @param array
// /// @param arr_size
// /// @return
// static int normalize(float *array, int arr_size)
// {
//     if (array == NULL || arr_size <= 0)
//         return -1;

//     float avg = 0;
//     for (int i = 0; i < arr_size; i++)
//         avg += (array[i] * array[i]);
//     avg = sqrtf(avg);

//     for (int i = 0; i < arr_size; i++)
//         array[i] /= avg;

//     return 0;
// }

static void lock_uav(void)
{
    /* 电机停转 */
    for (int i = 0; i < 4; i++)
        motor_run(i, 0);

    /* 清除PID的积分项 */
    yaw_angular_control.integral_sum = 0;
    roll_angular_control.integral_sum = 0;
    pitch_angular_control.integral_sum = 0;
}

static inline void range(float *origin, float min, float max)
{
    if (min > max)
        return;

    if (*origin < min)
        *origin = min;

    if (*origin > max)
        *origin = max;
}

/// @brief 滤波器初始化
/// @param
static void filter_init(void)
{
    biquad_filter_init_lpf(&biquad_pitch_ditem, 1000, 90);
    biquad_filter_init_lpf(&biquad_roll_ditem, 1000, 90);
    biquad_filter_init_lpf(&biquad_yaw_ditem, 1000, 90);
}

enum uav_status get_uav_arm_status(void)
{
    return uav_armed;
}

int set_uav_arm_status(enum uav_status new_status)
{
    uav_armed = new_status;
    bsp_led_operation(led1, (uav_armed == lock) ? off : on);

    if (uav_armed == lock)
        lock_uav();

    return 0;
}

int sbus_2_expect_attitude_thrust(float *expect_pitch, float *expect_roll, float *expect_yaw_angular, float *thrust)
{
    if (!expect_pitch || !expect_roll || !expect_yaw_angular)
        return RT_EINVAL;

    struct attitude_euler current_attitude;
    struct attitude_euler_angular current_angular;
    get_attitude(&current_attitude, &current_angular);

    struct sbus_frame frame;
    sbus_frame_get(&frame);
    if (frame.channel[0] == 0 && frame.channel[1] == 0)
        return RT_ERROR;
    /* 以HT-8A遥控器为例，SBUS解析通道值如下
     * 通道1：右侧摇杆左右方向，打到最左侧192对应滚转角期望的最大值40°，打到最右侧1792对应滚转角期望的最小值-40°
     * 通道2：右侧摇杆上下方向，打到最上侧192对应俯仰角期望的最小值-40°，打到最下侧1792对应滚转角期望的最大值40°
     * 通道3：左侧遥感上下方向，打到最下侧是最小值192，居中为992，打到最上侧是最大值1792 （油门）
     * 通道4：左侧摇杆左右方向，打到最左侧192对应偏航角速度期望最大值80°/s，打到最右侧1792对应偏航角速度期望的最小值-80°/s
     */

    /* 滚转角 */
    if (frame.channel[0] < SBUS_VAL_MIN)
        frame.channel[0] = SBUS_VAL_MIN;
    else if (frame.channel[0] > SBUS_VAL_MAX)
        frame.channel[0] = SBUS_VAL_MAX;
    *expect_roll = ((frame.channel[0] - SBUS_VAL_MIN) * (EXPECT_ROLL_MAX_DEGREE - 0) / (SBUS_VAL_MIN - SBUS_VAL_MID) + EXPECT_ROLL_MAX_DEGREE) / RAD2DEG;

    /* 俯仰角 */
    if (frame.channel[1] < SBUS_VAL_MIN)
        frame.channel[1] = SBUS_VAL_MIN;
    else if (frame.channel[1] > SBUS_VAL_MAX)
        frame.channel[1] = SBUS_VAL_MAX;
    *expect_pitch = ((frame.channel[1] - SBUS_VAL_MIN) * (EXPECT_PITCH_MAX_DEGREE - 0) / (SBUS_VAL_MAX - SBUS_VAL_MID) - EXPECT_PITCH_MAX_DEGREE) / RAD2DEG;

    *thrust = frame.channel[2];

    /* 偏航角速度 */
    if (frame.channel[3] < SBUS_VAL_MIN)
        frame.channel[3] = SBUS_VAL_MIN;
    else if (frame.channel[3] > SBUS_VAL_MAX)
        frame.channel[3] = SBUS_VAL_MAX;

    *expect_yaw_angular = ((frame.channel[3] - SBUS_VAL_MIN) * (EXPECT_YAW_ANGULAR_MAX_DEGREE - 0) / (SBUS_VAL_MIN - SBUS_VAL_MID) + EXPECT_YAW_ANGULAR_MAX_DEGREE) / RAD2DEG;

    return 0;
}

int control_mix(attitude_control_t *control_output, float *motor_pwm_cycle_arr)
{
    if (!control_output || !motor_pwm_cycle_arr)
        return -1;

    /* 此为X型四旋翼的混控分配方案 */
    /** X型四旋翼电机序号如下
     *     1               2
     *       \           /
     *         \       /
     *
     *          /     \
     *        /         \
     *      4             3
     */
    motor_pwm_cycle_arr[0] = 0.717f * control_output->roll + 0.717f * control_output->pitch - control_output->yaw + control_output->thrust;
    motor_pwm_cycle_arr[1] = -0.717f * control_output->roll + 0.717f * control_output->pitch + control_output->yaw + control_output->thrust;
    motor_pwm_cycle_arr[2] = -0.717f * control_output->roll - 0.717f * control_output->pitch - control_output->yaw + control_output->thrust;
    motor_pwm_cycle_arr[3] = 0.717f * control_output->roll - 0.717f * control_output->pitch + control_output->yaw + control_output->thrust;

    return 0;
}

static void control_attitude_2(void)
{
    static uint8_t cnt = 0;
    float roll_angular_out = 0;
    float pitch_angular_out = 0;
    float yaw_angular_out = 0;
    struct attitude_control control_output;

    /* 获取期望姿态 */
    float expect_roll, expect_pitch, expect_yaw_angular, thrust;
    sbus_2_expect_attitude_thrust(&expect_pitch, &expect_roll, &expect_yaw_angular, &thrust);

    /* 获取当前姿态 */
    struct attitude_euler current_attitude;
    struct attitude_euler_angular current_angular;
    get_attitude(&current_attitude, &current_angular);
    // rt_kprintf("expect roll %d, pitch %d, yaw_rate %d\n",
    //            (int)(expect_roll * RAD2DEG),
    //            (int)(expect_pitch * RAD2DEG),
    //            (int)(expect_yaw_angular * RAD2DEG));

    /* 每隔8次更新目标角速度（完成角度控制内环），即角度外环频率低于角速度内环 */
    ++cnt;
    if (cnt == 8)
    {
        cnt = 0;
        roll_angular_out = pid_calc(&roll_control, (expect_roll - current_attitude.roll));
        pitch_angular_out = pid_calc(&pitch_control, (expect_pitch - current_attitude.pitch));
        yaw_angular_out = expect_yaw_angular;

        if (roll_angular_out < 3.0f / RAD2DEG && roll_angular_out > -3.0f / RAD2DEG)
            roll_angular_out = 0;
        if (pitch_angular_out < 3.0f / RAD2DEG && pitch_angular_out > -3.0f / RAD2DEG)
            pitch_angular_out = 0;

        // rt_kprintf("roll_angular_out = %d, pitch_angular_out = %d\n",
        //            (int)roll_angular_out,
        //            (int)pitch_angular_out);
    }

    control_output.roll = pid_calc(&roll_angular_control, (roll_angular_out - current_angular.roll_angular));
    control_output.pitch = pid_calc(&pitch_angular_control, (pitch_angular_out - current_angular.pitch_angular));
    control_output.yaw = pid_calc(&yaw_angular_control, (yaw_angular_out - current_angular.yaw_angular));
    control_output.thrust = (thrust - SBUS_VAL_MIN) / (SBUS_VAL_MAX - SBUS_VAL_MIN) * THROTTLE_MAX;

    float motor_pwm_cycle_arr[4];
    float duty_cycle;
    /* 控制量分配 */
    control_mix(&control_output, motor_pwm_cycle_arr);
    // rt_kprintf("control output %d %d %d %d\n",
    //            (int)control_output.roll,
    //            (int)control_output.pitch,
    //            (int)control_output.yaw,
    //            (int)control_output.thrust);
    // rt_kprintf("motor run duty cycle %d %d %d %d\n",
    //            (int)motor_pwm_cycle_arr[0],
    //            (int)motor_pwm_cycle_arr[1],
    //            (int)motor_pwm_cycle_arr[2],
    //            (int)motor_pwm_cycle_arr[3]);

    /* 驱动电机 */
    for (int i = 0; i < 4; i++)
    {
        range(&motor_pwm_cycle_arr[i], MOTOR_PWM_RUN_MIN, MOTOR_PWM_MAX);
        duty_cycle = motor_pwm_cycle_arr[i] / MOTOR_PWM_MAX;
        motor_run(i, duty_cycle);
    }
}

/// @brief 当油门控制量小于5%时，电机停转
/// @param
/// @return 油门<5%时，返回1，否则返回0
static int throttle_dead_zone(void)
{
    struct sbus_frame frame;
    sbus_frame_get(&frame);
    float thrust = frame.channel[2]; /* 通道3是油门 */

    if ((thrust - SBUS_VAL_MIN) / (SBUS_VAL_MAX - SBUS_VAL_MIN) < 0.05f)
        return 1;

    return 0;
}

/// @brief 控制线程入口函数
/// @param params
static void control_thread_entry(void *params)
{
    motor_pwm_init();
    filter_init();

    while (1)
    {
        if (get_uav_arm_status() == lock || throttle_dead_zone())
        {
            lock_uav();
            rt_thread_mdelay(200);
            continue;
        }

        control_attitude_2();
        rt_thread_mdelay(5);
    }
}

int start_control_thread(void)
{
    rt_thread_t control_thread = rt_thread_create(
        "control_thread",
        control_thread_entry,
        RT_NULL,
        2048,
        8,
        20);

    if (!control_thread)
        return -1;

    rt_thread_startup(control_thread);
    return 0;
}
